#pragma once
// ============================================================================
// 文件：include/SLAM/attenuation_model.hpp
// 说明：基于“距离-强度”样本的衰减模型。
//      - 前期地标较少时：不用该模型，仅使用原有“按距离分桶（均值+方差）”阈值。
//      - 地标稳定后：累计确认地标的距离与强度样本，以分桶统计的均值/方差
//        作为该距离段的强度分布估计；阈值=均值 + k*标准差（k默认2.5），用来收紧
//        下一帧的高强过滤，降低远距离误检。
// 备注：非参数、鲁棒、可持续在线更新。
// ============================================================================

#include "SLAM/common.hpp"


namespace SLAM {

class AttenuationModel {
public:
  AttenuationModel() = default;

  // 添加一条确认地标样本（d：距离，i：强度）
  void add(double d, double i) {
    if (!std::isfinite(d) || !std::isfinite(i) || d <= 0) return;
    samples_.emplace_back(d, i);
    if (samples_.size() > max_samples_) samples_.pop_front();
    dirty_ = true;
  }

  // 给定距离 d，返回 I 的下界阈值（低于则视为非反光柱）（当模型准备就绪时才有效）
  double threshold(double d, double sigma_mult = 2.5) {
    if (!ready()) return std::numeric_limits<double>::infinity();
    maybeFit();
    double mu = predMean(d);
    double sd = predStd(d);
    return std::max(0.0, mu + sigma_mult * sd - sigma_mult*noise_);
  }

  // 模型是否可用（样本量足够并完成拟合）
  bool ready() const { return fitted_ && samples_.size() >= min_use_samples_; }
  size_t sampleCount() const { return samples_.size(); }
  void set_noise(double n){ noise_ = n; }
private:
  // 对采样进行分桶统计（0.5m 一桶），计算每桶均值/标准差
  void maybeFit() {
    if (!dirty_) return;
    bin_w_ = 0.5;
    bins_.clear();
    stats_.clear();

    for (auto &s : samples_) {
      int b = static_cast<int>(std::floor(s.first / bin_w_));
      bins_[b].push_back(s.second);
    }

    for (auto &kv : bins_) {
      auto &v = kv.second;
      if (v.size() < 6) continue; // 小样本桶跳过，避免方差不稳
      double m = 0;
      for (double z : v) m += z;
      m /= v.size();
      double var = 0;
      for (double z : v) { double d = z - m; var += d*d; }
      var = (v.size() > 1) ? var / (v.size() - 1) : 0.0;
      BinStat bs; bs.mean = m; bs.std = std::sqrt(std::max(0.0, var));
      stats_[kv.first] = bs;
    }
    fitted_ = !stats_.empty();
    dirty_  = false;
  }

  // 使用最近的桶来预测均值/标准差（没有插值，稳健为主）
  double predMean(double d) const {
    if (stats_.empty()) return std::numeric_limits<double>::infinity();
    int b = static_cast<int>(std::floor(d / bin_w_));
    auto it = stats_.find(b);
    if (it != stats_.end()) return it->second.mean;
    int best_key = 0; double best_dist = 1e9; bool found = false;
    for (auto &kv : stats_) {
      double dd = std::fabs(kv.first - b);
      if (dd < best_dist) { best_dist = dd; best_key = kv.first; found = true; }
    }
    if (!found) return std::numeric_limits<double>::infinity();
    return stats_.at(best_key).mean;
  }
  double predStd(double d) const {
    if (stats_.empty()) return 0.0;
    int b = static_cast<int>(std::floor(d / bin_w_));
    auto it = stats_.find(b);
    if (it != stats_.end()) return it->second.std;
    int best_key = 0; double best_dist = 1e9; bool found = false;
    for (auto &kv : stats_) {
      double dd = std::fabs(kv.first - b);
      if (dd < best_dist) { best_dist = dd; best_key = kv.first; found = true; }
    }
    if (!found) return 0.5;
    return stats_.at(best_key).std;
  }

  struct BinStat { double mean{0}, std{0}; };
  std::deque<std::pair<double,double>> samples_;
  static constexpr size_t max_samples_ = 5000;
  static constexpr size_t min_use_samples_ = 200; // 至少200个样本再启用模型
  double bin_w_{0.5};
  std::unordered_map<int, std::vector<double>> bins_;
  std::map<int, BinStat> stats_;
  bool dirty_{true};
  bool fitted_{false};
  double noise_{2.0};
};

} // namespace SLAM
